#ifndef _BITMAP_H
#define _BITMAP_H

#include <type.h>
#include <compiler.h>

static inline u32 bitmap_get(u32* bits, u32 idx) {
    return bits[idx / 32] & (1 << (idx % 32));
}

static inline void bitmap_set(u32* bits, u32 idx) {
    bits[idx / 32] |= (1 << (idx % 32));
}

static inline void bitmap_clear(u32* bits, u32 idx) {
    bits[idx / 32] &= ~(1 << (idx % 32));
}

static u32 bitmap_find_first_zero_from(u32* bits, u32 len, u32 start) {
    u32 offset = start % 32;
    u32 start_idx = start / 32;
    u32 count_32 = len / 32;
    u32 overflow = len % 32;

    for (u32 i = start_idx; i < count_32; i++) {
        u32 b = bits[i];
        if (i == start_idx) {
            b >>= offset;
            u32 mask = (1ULL << (32 - offset)) - 1;
            if (b == mask) {
                continue;
            }
            u32 idx = ctz(~b);
            return i * 32 + idx + offset;
        } else {
            if (b != 0xFFFFFFFF) {
                u32 idx = ctz(~b);
                return i * 32 + idx;
            }
        }
    }

    if (overflow) {
        if (start <= count_32 * 32) {
            offset = 0;
        }
        u32 b = bits[count_32];
        b >>= offset;
        u32 remaining = overflow - offset;
        if (remaining > 0) {
            if (b != (1 << remaining) - 1U) {
                u32 idx = ctz(~b);
                if (idx < remaining) {
                    return count_32 * 32 + idx + offset;
                }
            }
        }
    }

    return len;
}

static inline u32 bitmap_find_first_zero(u32* bits, u32 len) {
    return bitmap_find_first_zero_from(bits, len, 0);
}

#endif /* _BITMAP_H_ */